#include <conio.h>
#include <io_port.h>
#include <pic.h>

static uchar mirq1, mirq2;

void remap_pic(uchar master, uchar slave)
{
    outb(0x20, 0x11);
    outb(0xA0, 0x11);
    outb(0x21, master);
    outb(0xA1, slave);
    outb(0x21, 0x04);
    outb(0xA1, 0x02);
    outb(0x21, 0x01);
    outb(0xA1, 0x01);
    outb(0xA1, mirq2);
    outb(0x21, mirq1);
}

void mask_irq(uchar irqn)
{
    uchar tmp, tmp1;

    tmp = irqn & 0x07;
    tmp1 = ~(0x01 << tmp);

    if (irqn > 7) {
	irqn = inb(0xA1);
	irqn |= tmp1;
	outb(0xA1, irqn);
	if (irqn == PIC_IMR_ALL)
	    goto end_mask;
	tmp1 = PIC_IMR_IRQ2;
    }

    irqn = inb(0x21);
    irqn |= tmp1;
    outb(0x21, irqn);

  end_mask:
    return;
}

void unmask_irq(uchar irqn)
{
    uchar tmp, tmp1;

    tmp = irqn & 0x07;
    tmp1 = ~(0x01 << tmp);

    if (irqn > 7) {
	irqn = inb(0xA1);
	irqn &= tmp1;
	outb(0xA1, irqn);
	tmp1 = ~PIC_IMR_IRQ2;
    }

    irqn = inb(0x21);
    irqn &= tmp1;
    outb(0x21, irqn);
}

void send_eoirq(uchar irqn)
{
    if (irqn > 7)
	outb(0xA0, PIC_EOI);
    else
	outb(0x20, PIC_EOI);
}

uchar get_irqmask()
{
    uchar tmp;

    tmp = 0;
    tmp |= mirq2;
    tmp <<= 8;
    tmp |= mirq1;

    return (tmp);
}

void set_irqmask(uchar irqn)
{
    mirq2 = irqn >> 8 & 0xFF;
    mirq1 = irqn & 0xFF;
    outb(0xA1, mirq2);
    outb(0x21, mirq1);
}

void pic_init()
{
    uchar irq;

    kprintf("reprogramming PIC8259...");
    remap_pic(0x20, 0x28);

    for (irq = 0; irq < 16; irq++) {
	mask_irq(irq);
    }

    unmask_irq(2);
    unmask_irq(9);
    kprintf("Ok.\n");
}
